#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <netdb.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include "netif/lwbt/l2cap.h"
#include "netif/lwbt/hid.h"
#include "netif/lwbt/lwbt_memp.h"
#include "netif/lwbt/fcs.h"
#include "lwbtopts.h"
#include "lwip/debug.h"


// Global definitaion
struct bd_addr *in_progress_bdaddr;

int fifo_fd_Rx;
int fifo_fd_Tx;

struct bt_state {
  struct bd_addr bdaddr;
  struct pbuf *p;
} bt_hid_state;


err_t command_complete(void *arg, struct hci_pcb *pcb, u8_t ogf, u8_t ocf, u8_t result);
err_t our_l2ca_disconnect_ind(void *arg, struct l2cap_pcb *pcb, err_t err);
err_t our_l2ca_connect_ind(void *arg, struct l2cap_pcb *pcb, err_t err);
err_t read_bdaddr_complete(void *arg, struct bd_addr *bdaddr);
err_t pin_req(void *arg, struct bd_addr *bdaddr);
err_t inquiry_complete(void *arg, struct hci_pcb *pcb, struct hci_inq_res *ires, u16_t result);
err_t hid_connect(struct hid_pcb *pcb, /*u8_t cn,*/ err_t (* connected)(void *arg,
								    struct hid_pcb *tpcb,
								    err_t err));
struct hid_pcb* hid_new(struct l2cap_pcb *l2cappcb);
err_t l2cap_connected(void *arg, struct l2cap_pcb *l2cappcb, u16_t result, u16_t status);
err_t l2cap_disconnected_ind(void *arg, struct l2cap_pcb *pcb, err_t err);
void sdp_attributes_recv(void *arg, struct sdp_pcb *sdppcb, u16_t attribl_bc, struct pbuf *p);
err_t l2cap_disconnected_cfm(void *arg, struct l2cap_pcb *pcb);
err_t hid_input(void *arg, struct l2cap_pcb *l2cappcb, struct pbuf *p, err_t err);
err_t pin_reply(u8_t *pin_code, u8_t pinlen);

struct l2cap_pcb *l2cappcb_interrupt, *l2cappcb_control;
extern byte close_application;
byte is_shift_pressed = 0, previous_shift_state = 0, is_alt_pressed = 0,
	 is_ctrl_pressed = 0, is_fn_pressed = 0, is_cmd_pressed = 0,
	 previous_character = '\0';

typedef struct {
    uint8_t type;			/* always rfbKeyEvent */
    uint8_t down;			/* true if down (press), false if up */
    uint16_t pad;
    uint32_t key;			/* key is specified as an X keysym */
} rfbKeyEventMsg;

#define rfbClientSwap32IfLE(l) \
    ((((l) & 0xff000000) >> 24) | \
			     (((l) & 0x00ff0000) >> 8)  | \
			     (((l) & 0x0000ff00) << 8)  | \
			     (((l) & 0x000000ff) << 24))

int sock_handle, size;
struct sockaddr_in destination;
struct hostent *he;
char data[255];

void hid_init(void)
{
	if((he = gethostbyname("localhost")) == NULL)
	  {
	    printf("Error resolving host!\n");
	  }

	  if((sock_handle = socket(AF_INET, SOCK_STREAM, 0)) == -1)
	  {
	    printf("could not create socket!\n");
	  }

	  destination.sin_family = AF_INET;
	  destination.sin_port = htons(5900);
	  destination.sin_addr = *((struct in_addr *)he->h_addr);
	  memset(destination.sin_zero, 0, sizeof(destination.sin_zero));

	  if(connect(sock_handle, (struct sockaddr *)&destination, sizeof(struct sockaddr)) == -1)
	  {
	    printf("Error connecting to server!\n");
	  }

	  // Receive RFB init message from server first.
	  if(recv(sock_handle, data, sizeof(data), 0) == -1) printf("Server dead!! Didn't receive RFB init message!\n");
	  printf("Initializing connection... waiting for 10 seconds\n");
	  sleep(10);

	  // Send protocol version request to server.
	  sprintf(data, "RFB 003.008\n");
	  if(send(sock_handle, data, 12, 0) == -1) printf("Couldn't send \'Set protocol' message!\n");

	  // Get the security types supported
	  if((size = recv(sock_handle, data, sizeof(data), 0)) == -1) printf("Server dead!! Didn't receive RFB init message!\n");

	  // Send the 'No security' message.
	  data[0] = 1;
	  if(send(sock_handle, data, 1, 0) == -1) printf("Couldn't send \'Set No security\' message!\n");

	  // Receive server acknowledgement message.
	  if(size = (recv(sock_handle, data, sizeof(data), 0)) == -1) printf("Couldn't receive \'Security ack\' message!\n");

	  // Send 'ClientInit' message
	  data[0] = 0x01;
	  if(send(sock_handle, data, 1, 0) == -1) printf("Couldn't send \'ClientInit\' message!\n");

	  // Receive 'ServerInit' message
	  if((size = recv(sock_handle, data, sizeof(data), 0)) == -1) printf("Couldn't receive \'ServerInit\' message!\n");
}

void hid_start(void)
{
	hci_reset_all();
	l2cap_reset_all();
	sdp_reset_all();
	// HID reset is not used for now. Once its need is found this function will be implemented.
	// hid_reset_all();

	hci_cmd_complete(command_complete);
	hci_pin_req(pin_req);

	bt_hid_state.p = NULL;

	hci_reset();
}

/*-----------------------------------------------------------------------------------*/
/*
 * command_complete():
 *
 * Called by HCI when an issued command has completed during the initialization of the
 * host controller.
 * Initializes a search for other devices when host controller initialization is
 * completed.
 */
/*-----------------------------------------------------------------------------------*/
err_t
command_complete(void *arg, struct hci_pcb *pcb, u8_t ogf, u8_t ocf, u8_t result)
{
#ifdef DEBUG_LOW
  printf("\ninside command_complete...\n\n");
#endif
  switch(ogf) {
  case HCI_INFO_PARAM:
    switch(ocf) {
    case HCI_READ_BUFFER_SIZE:
      if(result == HCI_SUCCESS) {
	LWIP_DEBUGF(BT_IP_DEBUG, ("successful HCI_READ_BUFFER_SIZE.\n"));
#ifdef DEBUG_MED
	printf("successful HCI_READ_BUFFER_SIZE.\n");
#endif
	hci_read_bd_addr(read_bdaddr_complete);
      } else {
	LWIP_DEBUGF(BT_IP_DEBUG, ("Unsuccessful HCI_READ_BUFFER_SIZE.\n"));
	printf("Unsuccessful HCI_READ_BUFFER_SIZE.\n");
	return ERR_CONN;
      }
      break;
    case HCI_READ_BD_ADDR:
      if(result == HCI_SUCCESS) {
	LWIP_DEBUGF(BT_IP_DEBUG, ("successful HCI_READ_BD_ADDR.\n"));
#ifdef DEBUG_MED
	printf("successful HCI_READ_BD_ADDR.\n");
#endif
	//hci_set_event_filter(0x01, 0x01, cod_lap_dun); /* Report only devices with a specific type of CoD */


	hci_set_event_filter(0x00, 0x00, 0); /* clear event filters (???) */


      } else {
	LWIP_DEBUGF(BT_IP_DEBUG, ("Unsuccessful HCI_READ_BD_ADDR.\n"));
	printf("Unsuccessful HCI_READ_BD_ADDR.\n");
	return ERR_CONN;
      }
      break;
    default:
      LWIP_DEBUGF(BT_IP_DEBUG, ("Unknown HCI_INFO_PARAM command complete event\n"));
      printf("Unknown HCI_INFO_PARAM command complete event\n");
      break;
    }
    break;
  case HCI_HC_BB_OGF:
    switch(ocf) {
    case HCI_RESET:
      if(result == HCI_SUCCESS) {
	LWIP_DEBUGF(BT_IP_DEBUG, ("successful HCI_RESET.\n"));
#ifdef DEBUG_MED
	printf("successful HCI_RESET.\n");
#endif
	hci_read_buffer_size();
      } else {
	LWIP_DEBUGF(BT_IP_DEBUG, ("Unsuccessful HCI_RESET.\n"));
	printf("Unsuccessful HCI_RESET.\n");
	return ERR_CONN;
      }
      break;
    case HCI_SET_EVENT_FILTER:
      if(result == HCI_SUCCESS) {
	LWIP_DEBUGF(BT_IP_DEBUG, ("successful HCI_SET_EVENT_FILTER.\n"));
#ifdef DEBUG_MED
	printf("successful HCI_SET_EVENT_FILTER.\n");
#endif
	hci_write_page_timeout(0x4000); /* 10.24s */
      } else {
	LWIP_DEBUGF(BT_IP_DEBUG, ("Unsuccessful HCI_SET_EVENT_FILTER.\n"));
	printf("Unsuccessful HCI_SET_EVENT_FILTER.\n");
	return ERR_CONN;
      }
      break;
    case HCI_READ_LOC_NAME:
      if(result == HCI_SUCCESS) {
          printf(" processing local name....\n");
      }
    case HCI_WRITE_PAGE_TIMEOUT:
      if(result == HCI_SUCCESS) {
	LWIP_DEBUGF(BT_IP_DEBUG, ("successful HCI_WRITE_PAGE_TIMEOUT.\n"));
#ifdef DEBUG_MED
	printf("successful HCI_WRITE_PAGE_TIMEOUT.\n");
#endif
	hci_cmd_complete(NULL); /* Initialization done, don't come back */
	//hci_connection_complete(acl_conn_complete);
	LWIP_DEBUGF(BT_IP_DEBUG, ("Initialization done.\n"));
#ifdef DEBUG_MED
	printf("Initialization done.\n");
#endif
	LWIP_DEBUGF(BT_IP_DEBUG, ("Discover other Bluetooth devices.\n"));
	//printf("Discover other Bluetooth devices(?).\n");
	//hci_inquiry2(0x009E8B33, 0x0A, 0x08, inquiry_complete); //TODO: FAILED????
        //hci_read_local_name();
        //printf("payload in after read_local_name = %d \n", *((u16_t *)cb->p->payload));
        //phybusif_input(cb); /* Check for input ???? */
      } else {
	LWIP_DEBUGF(BT_IP_DEBUG, ("Unsuccessful HCI_WRITE_PAGE_TIMEOUT.\n"));
	printf("Unsuccessful HCI_WRITE_PAGE_TIMEOUT.\n");
	return ERR_CONN;
      }
      break;
    default:
      LWIP_DEBUGF(BT_IP_DEBUG, ("Unknown HCI_HC_BB_OGF command complete event\n"));
      printf("Unknown HCI_HC_BB_OGF command complete event\n");
      break;
    }
    break;
  default:
    LWIP_DEBUGF(BT_IP_DEBUG, ("Unknown command complete event. OGF = 0x%x OCF = 0x%x\n", ogf, ocf));
    printf("Unknown command complete event. OGF = 0x%x OCF = 0x%x\n", ogf, ocf);
    break;
  }
  return ERR_OK;
}

/*-----------------------------------------------------------------------------------*/
/*
 * our_l2ca_disconnect_ind():
 *
 * Called when l2cap disonnection request was received. (modeled after bt_disconnect_ind from bt_ip_lap.c)
 */
/*-----------------------------------------------------------------------------------*/
err_t
our_l2ca_disconnect_ind(void *arg, struct l2cap_pcb *pcb, err_t err)
{
#ifdef DEBUG_LOW
	printf("inside our_l2ca_disconnect_ind\n");
#endif

	if(pcb->psm == SDP_PSM) {
		sdp_lp_disconnected(pcb);
	} else if(pcb->psm == HID_Control_PSM) {
		//ret = rfcomm_lp_disconnected(pcb);
	}

	l2cap_close(pcb);

	return ERR_OK;
}

/*-----------------------------------------------------------------------------------*/
/*
 * our_l2ca_connect_ind():
 *
 * Called when l2cap connection request was received. (modeled after bt_connect_ind from bt_ip_lap.c)
 */
/*-----------------------------------------------------------------------------------*/
err_t
our_l2ca_connect_ind(void *arg, struct l2cap_pcb *pcb, err_t err)
{
#ifdef DEBUG_LOW
	printf("inside our_l2ca_connect_ind ...\n");
#endif
	printf("inside our_l2ca_connect_ind ...\n");

	/* Tell L2CAP that we wish to be informed of a disconnection request */
	l2cap_disconnect_ind(pcb, our_l2ca_disconnect_ind);

	/* Tell L2CAP that we wish to be informed of incoming data */
	if(pcb->psm == SDP_PSM) {
		l2cap_recv(pcb, sdp_recv);
	} else if (pcb->psm == HID_Control_PSM) {
		l2cap_recv(pcb, hid_input);
	}

	return ERR_OK;
}

/*-----------------------------------------------------------------------------------*/
/*
 * read_bdaddr_complete():
 *
 * Called by HCI when a read local bluetooth device address complete event was received.
 */
/*-----------------------------------------------------------------------------------*/
err_t
read_bdaddr_complete(void *arg, struct bd_addr *bdaddr)
{
  memcpy(&(bt_hid_state.bdaddr), bdaddr, 6);

  int i;
  printf("\nlocal BD_ADDR = ");
  for (i=0; i<6; i++) {
    local_mac_addr[i] = bdaddr[0].addr[5-i];
    printf("%02X", bdaddr[0].addr[5-i]);
    if (i<5) {printf(":");}
  }
  printf("\n\n");

  return ERR_OK;
}

/*-----------------------------------------------------------------------------------*/
/*
 * pin_reply():
 *
 * Continue w/ HID connect process with specified pin
 */
/*-----------------------------------------------------------------------------------*/
err_t
pin_reply(u8_t *pin_code, u8_t pinlen) {

	printf("global bd_addr = %02X:%02X:%02X:%02X:%02X:%02X\n", in_progress_bdaddr[0].addr[5], in_progress_bdaddr[0].addr[4], in_progress_bdaddr[0].addr[3], in_progress_bdaddr[0].addr[2], in_progress_bdaddr[0].addr[1], in_progress_bdaddr[0].addr[0]);
	printf("  pin_code = %s   pinlen = %d \n", pin_code, pinlen);

	err_t return_code;
	return_code = hci_pin_code_request_reply(in_progress_bdaddr, pinlen, ((u8_t *) pin_code));
	return 0;
}

/*-----------------------------------------------------------------------------------*/
/*
 * pin_req():
 *
 * Called by HCI when a request for a PIN code has been received. A PIN code is
 * required to create a new link key.
 * Replys to the request with the given PIN code
 *
 */
/*-----------------------------------------------------------------------------------*/
err_t
pin_req(void *arg, struct bd_addr *bdaddr)
{
  printf("\tChecking PIN\n");
  sprintf((char *) pin_code, "1111");
#ifdef DEBUG_MED
  printf("    using pin_code = %s \n", pin_code);
#endif
  in_progress_bdaddr = bdaddr;

  if (lib_interactive_pin == 0) {
	  printf("Sending pin code: %s\n", pin_code);
  } else {
    // clear lib_is_scanning flag to (temporarily) stop rfcomm connection
    // but respond w/ pin code request by calling pin_reply(), see above)
    lib_is_connecting = 0;
    printf("using pin_code = %s \n", pin_code);
  }
  return hci_pin_code_request_reply(bdaddr, 4, ((u8_t *) pin_code));
}

/*-----------------------------------------------------------------------------------*/
/*
 * inquiry_complete():
 *
 * Called by HCI when a inquiry complete event was received.
 * Connects to the first device in the list.
 * Initializes a search for other devices if the inquiry failed.
 */
/*-----------------------------------------------------------------------------------*/
err_t
inquiry_complete(void *arg, struct hci_pcb *pcb, struct hci_inq_res *ires, u16_t result)
{
  struct l2cap_pcb *l2cappcb;

#ifdef DEBUG_MED
  printf("\ninside inquiry complete...\n\n");
#endif
  if(result == HCI_SUCCESS) {
#ifdef DEBUG_LOW
    printf("\n  HCI_SUCCESS ...\n");
#endif
    LWIP_DEBUGF(BT_IP_DEBUG, ("successful Inquiry\n"));
    if(ires != NULL) {
      LWIP_DEBUGF(BT_IP_DEBUG, ("Initiate L2CAP connection\n"));
#ifdef DEBUG_LOW
      //printf("Initiate L2CAP connection (?)\n");
      LWIP_DEBUGF(BT_IP_DEBUG, ("ires->psrm %d\n ires->psm %d\n ires->co %d\n", ires->psrm, ires->psm, ires->co));
      LWIP_DEBUGF(BT_IP_DEBUG, ("ires->bdaddr 0x%x:0x%x:0x%x:0x%x:0x%x:0x%x\n", ires->bdaddr.addr[5], ires->bdaddr.addr[4], ires->bdaddr.addr[3], ires->bdaddr.addr[2], ires->bdaddr.addr[1], ires->bdaddr.addr[0]));
#endif

//      printf("ires->cod[1] & 0x1F = %d \n", (ires->cod[1] & 0x1F));

      if((l2cappcb = l2cap_new()) == NULL) {
	LWIP_DEBUGF(BT_IP_DEBUG, ("inquiry_complete: Could not alloc L2CAP pcb\n"));
	printf(" -----> inquiry_complete: Could not alloc L2CAP pcb\n");
	return ERR_MEM;
      }

#ifdef DEBUG_LOW
//      u8_t count;
//      printf("ires->bdaddr = ");
//      for (count = 0; count < 6; count++) {
//          printf("%02X", ires->bdaddr.addr[5-count]);
//          if (count < 5) {printf(":");}
//      }
//      printf("\n");
#endif
    } else {
#ifdef DEBUG_LOW
      printf("ires = NULL ?????\n");
#endif
      //hci_inquiry2(0x009E8B33, 0x04, 0x02, inquiry_complete);
    }
  } else {
    LWIP_DEBUGF(BT_IP_DEBUG, ("Unsuccessful Inquiry.\n"));
    printf("Unsuccessful Inquiry.\n");
    //hci_inquiry2(0x009E8B33, 0x04, 0x01, inquiry_complete);
  }

  // clear the lib_is_scanning flag to let library know hci scan is complete

  lib_is_scanning=0;
  if (lib_threaded == 1) {
    if (lib_callbacks->scan_complete != NULL) {
      lib_callbacks->scan_complete();
    }
  }

  return ERR_OK;
}

/*-----------------------------------------------------------------------------------*/
/*
 * rfcomm_connect():
 *
 * Sends a RFCOMM start asynchronous balanced mode frame to startup the channel. Also
 * specify the function to be called when the channel has been connected.
 */
/*-----------------------------------------------------------------------------------*/
err_t
hid_connect(struct hid_pcb *pcb, /*u8_t cn,*/ err_t (* connected)(void *arg,
								    struct hid_pcb *tpcb,
								    err_t err))
{
  //struct rfcomm_hdr *hdr;
  struct pbuf *p;
  err_t ret;
  //struct hid_pcb *tpcb;

  LWIP_DEBUGF(RFCOMM_DEBUG, ("hid_connect\n"));
#ifdef DEBUG_MED
  printf("\ninside hid_connect...\n\n");
#endif

  pcb->connected = connected;

  //printf("TPCB value: 0x%08X \n", tpcb);
  //if(tpcb == NULL) {
  //	pcb = hid_new(pcb->l2cappcb);
  //}

  printf("Trying to connect to the device...\nNothing doing for now!\n\n");
  // The code below will be used to create a control channel message to connect to the keyboard.
  /*
  if((p = pbuf_alloc(PBUF_RAW, RFCOMM_SABM_LEN, PBUF_RAM)) == NULL) {
    // Could not allocate memory for pbuf
    LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_connect: Could not allocate memory for pbuf\n"));
    return ERR_MEM;
  }

  hdr = p->payload;
  hdr->addr = (1 << 0) | ((pcb->rfcommcfg & RFCOMM_CFG_IR) << 1) | (((pcb->rfcommcfg & RFCOMM_CFG_IR) ^ 1) << 2) | (pcb->cn << 3);
  hdr->ctrl = RFCOMM_SABM;
  hdr->len = (1 << 0) | (0 << 1);
  ((u8_t *)p->payload)[RFCOMM_HDR_LEN_1] = fcs8_crc_calc(p, RFCOMM_CRC_CHECK_LEN);


  if((ret = l2ca_datawrite(pcb->l2cappcb, p)) == ERR_OK) {
    printf("\n okay, thats good ...\n");
  }
*/

  pbuf_free(p);

  return ret;
}

/*-----------------------------------------------------------------------------------*/
/*
 * hid_new():
 *
 * Creates a new hid protocol control block but doesn't place it on
 * any of the hid PCB lists.
 */
/*-----------------------------------------------------------------------------------*/
struct hid_pcb *
hid_new(struct l2cap_pcb *l2cappcb)
{
  struct hid_pcb *pcb;

  pcb = lwbt_memp_malloc(MEMP_HID_PCB);
  if(pcb != NULL) {
    memset(pcb, 0, sizeof(struct hid_pcb));
    pcb->l2cappcb = l2cappcb;
    return pcb;
  }
  LWIP_DEBUGF(RFCOMM_DEBUG, ("rfcomm_new: Could not allocate a new pcb\n"));
  return NULL;
}

/*-----------------------------------------------------------------------------------*/
/*
 * l2cap_connected():
 *
 * Called by L2CAP when a connection response was received.
 * Sends a L2CAP configuration request.
 * Initializes a search for other devices if the connection attempt failed.
 */
/*-----------------------------------------------------------------------------------*/
err_t
l2cap_connected(void *arg, struct l2cap_pcb *l2cappcb, u16_t result, u16_t status)
{
  struct sdp_pcb *sdppcb;
  //struct hid_pcb *hidpcb;

#ifdef DEBUG_MED
  printf("\ninside l2cap_connected!!! \n\n");
#endif
  //printf("\ninside l2cap_connected!!! \n\n");
  u8_t ssp[] = {0x35, 0x03, 0x19, 0x11, 0x24}; /* Service search pattern with HID UUID is default */
  u16_t max_attrib_byte_cnt = 0x0190;

  err_t ret;

  // 0000 1010

  /* Attribute IDs to search for in data element sequence form */
  u8_t attrids[] = {
					  0x35, 0x12,
						0x0A, 0x00, 0x00, 0x00, 0x01, /* AttributeIDRange */
						0x09, 0x00, 0x06, /* AttributeID */
						0x0A, 0x01, 0x00, 0x01, 0x02, /* AttributeIDRange */
						0x0A, 0x02, 0x00, 0x02, 0x0C /* AttributeID */
				   };

  if(result == L2CAP_CONN_SUCCESS) {
    LWIP_DEBUGF(BT_IP_DEBUG, ("l2cap_connected: L2CAP connected pcb->state = %d\n", l2cappcb->state));
    /* Tell L2CAP that we wish to be informed of a disconnection request */
    l2cap_disconnect_ind(l2cappcb, l2cap_disconnected_ind);

    //printf("L2CAP PSM: 0x%04X\n", l2cap_psm(l2cappcb));
    switch(l2cap_psm(l2cappcb)) {
    case SDP_PSM:
      LWIP_DEBUGF(BT_IP_DEBUG, ("l2cap_connected: SDP L2CAP configured. Result = %d\n", result));
      //printf(" \n\n hey there, inside SDP_PSM case!!! \n");

      if((sdppcb = sdp_new(l2cappcb)) == NULL) {
	LWIP_DEBUGF(BT_IP_DEBUG, ("l2cap_connected: Failed to create a SDP PCB\n"));
	//printf("l2cap_connected: Failed to create a SDP PCB\n");
	return ERR_MEM;
      }

      //printf("Vinay: Calling sdp_serv thru l2cap_recv\n");
      l2cap_recv(l2cappcb, sdp_recv);

      //printf("Initiating SSA request...\n");
      ret = sdp_service_search_attrib_req(sdppcb, max_attrib_byte_cnt, ssp, sizeof(ssp), attrids, sizeof(attrids),
					  sdp_attributes_recv);
      return ret;

    case HID_Control_PSM:
      //printf("handling HID PSM\n");
      l2cappcb_control = l2cappcb;
      LWIP_DEBUGF(BT_IP_DEBUG, ("l2cap_connected: HID L2CAP configured. Result = %d CN = %d\n", result, bt_ip_state.cn));
      //printf(" \n\n hey there, inside HID_Control_PSM case!!! \n");
      // Send a request to open HID Interrupt Channel.
      if((l2cappcb_interrupt = l2cap_new()) == NULL) {
            LWIP_DEBUGF(BT_IP_DEBUG, ("sdp_attributes_recv: Could not alloc L2CAP pcb\n"));
            return;
          }
      l2ca_connect_req(l2cappcb_interrupt, &(l2cappcb->remote_bdaddr), HID_Interrupt_PSM, 0, l2cap_connected);
      //printf("Starting HID_Interrupt connection...\n");

    case HID_Interrupt_PSM:
    	//printf(" \n\n hey there, inside HID_Interrupt_PSM case!!! \n");
    	l2cap_recv(l2cappcb_interrupt, hid_input);
      /*
      if((rfcommpcb = rfcomm_new(l2cappcb)) == NULL) {
	LWIP_DEBUGF(BT_IP_DEBUG, ("l2cap_connected: Failed to create a RFCOMM PCB\n"));
	printf("l2cap_connected: Failed to create a RFCOMM PCB\n");
	return ERR_MEM;
      }

      hci_link_key_not(link_key_not); // Set function to be called if a new link key is created
	  */


      err_t temp;
      temp = 0;

      return temp;
    default:
    	//printf("l2cap_connected: Unknown PSM\n");
      return ERR_VAL;
    }
  } else {
	  //printf("l2cap_connected: L2CAP not connected. Redo inquiry\n");
    LWIP_DEBUGF(BT_IP_DEBUG, ("l2cap_connected: L2CAP not connected. Redo inquiry\n"));
    l2cap_close(l2cappcb);

  }

  return ERR_OK;
}

/*-----------------------------------------------------------------------------------*/
/*
* l2cap_disconnected_ind():
*
* Called by L2CAP to indicate that remote L2CAP protocol disconnected.
* Disconnects the HID protocol and the ACL link before it initializes a search for
* other devices.
*
*/
/*-----------------------------------------------------------------------------------*/
err_t
l2cap_disconnected_ind(void *arg, struct l2cap_pcb *pcb, err_t err)
{
  err_t ret = ERR_OK;

  LWIP_DEBUGF(BT_IP_DEBUG, ("l2cap_disconnected_ind: L2CAP disconnected\n"));

  if(pcb->psm == SDP_PSM) {
    sdp_lp_disconnected(pcb);
    l2cap_close(pcb);
  } else if(pcb->psm == HID_Control_PSM) {
    //ret = rfcomm_lp_disconnected(pcb);
    /* We can do this since we know that we are the only channel on the ACL link. If ACL link already is
     down we get an ERR_CONN returned */
    hci_disconnect(&(pcb->remote_bdaddr), HCI_OTHER_END_TERMINATED_CONN_USER_ENDED);
    l2cap_close(pcb);
    //bt_rfcomm_start();
  }

  return ret;
}

/*-----------------------------------------------------------------------------------*/
/*
 * sdp_attributes_recv():
 *
 * Can be used as a callback by SDP when a response to a service attribute request or
 * a service search attribute request was received.
 * Disconnects the L2CAP SDP channel and connects to the RFCOMM one.
 * If no RFCOMM channel was found it initializes a search for other devices.
 */
/*-----------------------------------------------------------------------------------*/
void
sdp_attributes_recv(void *arg, struct sdp_pcb *sdppcb, u16_t attribl_bc, struct pbuf *p)
{
  struct l2cap_pcb *l2cappcb;

  printf("\n\n SSAR Resp: holy crap, ive received an sdp response..... \n\n");
#ifdef DEBUG_HIGH
  printf("\n\n   Received viable sdp request, now to disconnect\n");
  printf("   and find channel to start rfcomm... \n\n");
#endif

  printf("SSAR Resp: Disconnecting from existing L2CAP channel\n");
  l2ca_disconnect_req(sdppcb->l2cappcb, l2cap_disconnected_cfm);

  if((l2cappcb = l2cap_new()) == NULL) {
      LWIP_DEBUGF(BT_IP_DEBUG, ("sdp_attributes_recv: Could not alloc L2CAP pcb\n"));
      return;
    }
  printf("SSAR Resp: New L2CAP channel created\n");
  // Start the HID connection by first makeing a l2cap connection
  // and retreiving pertinant sdp service records; if a viable record
  // is found and w/ an appropriate (non-zero?) channel then the connection is made
  printf("Attempting connection to device (");
  int i;
  for (i=0;i<6;i++)
  {
	  printf("0x%02X", sdppcb->l2cappcb->remote_bdaddr.addr[i]);
  }
  printf(")\n");
  l2ca_connect_req(l2cappcb, &(sdppcb->l2cappcb->remote_bdaddr), HID_Control_PSM, 0, l2cap_connected);
  printf("Starting HID connection...\n");

  printf("L2CAP Connection request sent...\n");
  printf("SSAR Resp: New L2CAP channel connected for HID\n");

  sdp_free(sdppcb);
}

/*-----------------------------------------------------------------------------------*/
/*
 * l2cap_disconnected_cfm():
 *
 * Called by L2CAP to confirm that the L2CAP disconnection request was successful
 *
 */
/*-----------------------------------------------------------------------------------*/
err_t l2cap_disconnected_cfm(void *arg, struct l2cap_pcb *pcb)
{
  LWIP_DEBUGF(BT_IP_DEBUG, ("l2cap_disconnected_cfm\n"));
#ifdef DEBUG_MED
  printf("inside l2cap_disconnected_cfm ...\n");
#endif
  l2cap_close(pcb);
  return ERR_OK;
}

/*-----------------------------------------------------------------------------------*/
/*
 * hid_input():
 *
 * Called by the lower layer. Does a frame check, parses the header and forward it to
 * the upper layer or handle the command frame.
 */
/*-----------------------------------------------------------------------------------*/

err_t
hid_input(void *arg, struct l2cap_pcb *l2cappcb, struct pbuf *p, err_t err)
{
	short characterCode;
	// The first byte is always 0xA1.
	// if the 2nd byte is 0x01, then regular keys are being pressed.
	// if the 2nd byte is 0x11 then special function keys are pressed.
	// The third byte is a bit-map that indicates that modifier buttons (alt, ctrl, shift, etc.) are pressed.
	// If the 3rd byte is 0x08 then its the "eject" button. In that case we need to stop looping through 'check_input'.
	// 5th byte till 10th byte indicate the keys pressed (in the sequence of press).

	// Send 'KeyEvent' message
	rfbKeyEventMsg ke;
	ke.type = 0x04;
	ke.down = 1;

	fflush(stdout);
	if(((u8_t *)p->payload)[0] == 0xA1)
	{
		if(((u8_t *)p->payload)[1] == 0x11)
		{
			if(((u8_t *)p->payload)[2] == 0x08) // If eject key was pressed!
			{
				// exit the application
				close_application = 1;
				return ERR_OK;
			}
			else if(((u8_t *)p->payload)[2] == 0x10) // ---- <fn> ----
			{
				//printf("<fn>");
				is_fn_pressed = 1;
			}
			else
			{
				is_fn_pressed = 0;
			}
		}

		// Now analyze the keys pressed and printf them on the screen.
		//printf("Keyboard keys: ");
		if(((u8_t *)p->payload)[1] == 0x01) // If the 2nd byte was 0x01, it represents regular keys
		{
			// First the modifier keys
			previous_shift_state = is_shift_pressed;
			if(((u8_t *)p->payload)[2] & 0x02) // ---- <left shift> ----
				{is_shift_pressed = 1;}
			else if(((u8_t *)p->payload)[2] & 0x20) // ---- <right shift> ----
				{is_shift_pressed = 1;}
			else
				{is_shift_pressed = 0;}

			if(((u8_t *)p->payload)[2] & 0x04) // ---- <left alt> ----
				{is_alt_pressed = 1;}
			else if(((u8_t *)p->payload)[2] & 0x40) // ---- <right alt> ----
				{is_alt_pressed = 1;}
			else
				{is_alt_pressed = 0;}

			if(((u8_t *)p->payload)[2] & 0x08) // ---- <left command> ----
				{is_cmd_pressed = 1;}
			else if(((u8_t *)p->payload)[2] & 0x80) // ---- <right command> ----
				{is_cmd_pressed = 1;}
			else
				{is_cmd_pressed = 0;}

			// ---- <control> ----
			if(((u8_t *)p->payload)[2] & 0x01)
				{is_ctrl_pressed = 1;}
			else
				{is_ctrl_pressed = 0;}

			// Now the regular keys
			//int i;
			//for(i = 0; i < 6; i++)
			//{
			// If the previous shift state was '1' and the current state is '0' then
			// the user has just release the shift key. 
			// This code is to avoid two characters (one capital and one normal) being posted for a single key press
			if(previous_shift_state != is_shift_pressed)
			{
				return ERR_OK;
			}
			else if(previous_character == ((u8_t *)p->payload)[4])
			{
				((u8_t *)p->payload)[4] = 0x00;
			}

				switch(((u8_t *)p->payload)[4])
				{
				case 0x29: // escape key
					characterCode = 0x001B;
					break;
				case 0x3A:
					if(is_fn_pressed == 1) characterCode = 0xFFBE;
					//else printf("<reduce brightness>");
					break;
				case 0x3B:
					if(is_fn_pressed == 1) characterCode = 0xFFBF;
					//else printf("<increase brightness>");
					break;
				case 0x3C:
					if(is_fn_pressed == 1) characterCode = 0xFFC0;
					//else printf("<Expose>");
					break;
				case 0x3D:
					if(is_fn_pressed == 1) characterCode = 0xFFC1;
					//else printf("<Dashboard>");
					break;
				case 0x3E:
					if(is_fn_pressed == 1) characterCode = 0xFFC2;
					//else printf("<Fn_F5>");
					break;
				case 0x3F:
					if(is_fn_pressed == 1) characterCode = 0xFFC3;
					//else printf("<Fn_F6>");
					break;
				case 0x40:
					if(is_fn_pressed == 1) characterCode = 0xFFC4;
					//else printf("<Rewind / Previous track>");
					break;
				case 0x41:
					if(is_fn_pressed == 1) characterCode = 0xFFC5;
					//else printf("<Play / Pause>");
					break;
				case 0x42:
					if(is_fn_pressed == 1) characterCode = 0xFFC6;
					//else printf("<Forward / Next track>");
					break;
				case 0x43:
					if(is_fn_pressed == 1) characterCode = 0xFFC7;
					//else printf("<Mute>");
					break;
				case 0x44:
					if(is_fn_pressed == 1) characterCode = 0xFFC8;
					//else printf("<Increase volume>");
					break;
				case 0x45:
					if(is_fn_pressed == 1) characterCode = 0xFFC9;
					//else printf("<Reduce volume>");
					break;
				case 0x35:
					if(is_shift_pressed == 1) characterCode = 0x007E; // ~
					else characterCode = 0x0060; // `
					break;
				case 0x1E:
					if(is_shift_pressed == 1) characterCode = 0x0021;// !
					else characterCode = 0x0031;// 1
					break;
				case 0x1F:
					if(is_shift_pressed == 1) characterCode = 0x0040; // @
					else characterCode = 0x0032; // 2
					break;
				case 0x20:
					if(is_shift_pressed == 1) characterCode = 0x0023; // #
					else characterCode = 0x0033;// 3
					break;
				case 0x21:
					if(is_shift_pressed == 1) characterCode = 0x0024;// $
					else characterCode = 0x0034; // 4
					break;
				case 0x22:
					if(is_shift_pressed == 1) characterCode = 0x0025;// %
					else characterCode = 0x0035;// 5
					break;
				case 0x23:
					if(is_shift_pressed == 1) characterCode = 0x005E;// ^
					else characterCode = 0x0036;// 6
					break;
				case 0x24:
					if(is_shift_pressed == 1) characterCode = 0x0026;// &
					else characterCode = 0x0037;// 7
					break;
				case 0x25:
					if(is_shift_pressed == 1) characterCode = 0x002A; // *
					else characterCode = 0x0038; // 8
					break;
				case 0x26:
					if(is_shift_pressed == 1) characterCode = 0x0028; // (
					else characterCode = 0x0039; // 9
					break;
				case 0x27:
					if(is_shift_pressed == 1) characterCode = 0x0029;// )
					else characterCode = 0x0030; // 0
					break;
				case 0x2D:
					if(is_shift_pressed == 1) characterCode = 0x005F;// _
					else characterCode = 0x002D; // -
					break;
				case 0x2E:
					if(is_shift_pressed == 1) characterCode = 0x002B;// +
					else characterCode = 0x003D; // =
					break;
				case 0x2A:
					if(is_shift_pressed == 1) characterCode = 0xFFFF; // <shift><delete> i.e., <delete>
					else characterCode = 0x007F;// <backspace>
					break;
				case 0x2B:
					characterCode = 0xFF09; // <tab>
					break;
				case 0x14:
					if(is_shift_pressed == 1) characterCode = 0x0051;// Q
					else characterCode = 0x0071; // q
					break;
				case 0x1A:
					if(is_shift_pressed == 1)characterCode = 0x0057; // W
					else characterCode = 0x0077; // w
					break;
				case 0x08:
					if(is_shift_pressed == 1) characterCode = 0x0045;// E
					else characterCode = 0x0065;// e
					break;
				case 0x15:
					if(is_shift_pressed == 1) characterCode = 0x0052;// R
					else characterCode = 0x0072;// r
					break;
				case 0x17:
					if(is_shift_pressed == 1) characterCode = 0x0054;// T
					else characterCode = 0x0074;// t
					break;
				case 0x1C:
					if(is_shift_pressed == 1) characterCode = 0x0059;// Y
					else characterCode = 0x0079;// y
					break;
				case 0x18:
					if(is_shift_pressed == 1) characterCode = 0x0055;// U
					else characterCode = 0x0075;// u
					break;
				case 0x0C:
					if(is_shift_pressed == 1) characterCode = 0x0049;// I
					else characterCode = 0x0069;// i
					break;
				case 0x12:
					if(is_shift_pressed == 1) characterCode = 0x004F;// O
					else characterCode = 0x006F;// o
					break;
				case 0x13:
					if(is_shift_pressed == 1) characterCode = 0x0050;// P
					else characterCode = 0x0070;// p
					break;
				case 0x2F:
					if(is_shift_pressed == 1) characterCode = 0x007B;// {
					else characterCode = 0x005B;// [
					break;
				case 0x30:
					if(is_shift_pressed == 1) characterCode = 0x007D;// }
					else characterCode = 0x005D;// ]
					break;
				case 0x31:
					if(is_shift_pressed == 1) characterCode = 0x007C;// |
					else characterCode = 0x005C;/* \ */
					break;
				case 0x04:
					if(is_shift_pressed == 1) characterCode = 0x0041;// A
					else characterCode = 0x0061;// a
					break;
				case 0x16:
					if(is_shift_pressed == 1) characterCode = 0x0053;// S
					else characterCode = 0x0073;// s
					break;
				case 0x07:
					if(is_shift_pressed == 1) characterCode = 0x0044;// D
					else characterCode = 0x0064;// d
					break;
				case 0x09:
					if(is_shift_pressed == 1) characterCode = 0x0046;// F
					else characterCode = 0x0066;// f
					break;
				case 0x0A:
					if(is_shift_pressed == 1) characterCode = 0x0047;// G
					else characterCode = 0x0067;// g
					break;
				case 0x0B:
					if(is_shift_pressed == 1) characterCode = 0x0048;// H
					else characterCode = 0x0068;// h
					break;
				case 0x0D:
					if(is_shift_pressed == 1) characterCode = 0x004A;// J
					else characterCode = 0x006A;// j
					break;
				case 0x0E:
					if(is_shift_pressed == 1) characterCode = 0x004B;// K
					else characterCode = 0x006B;// k
					break;
				case 0x0F:
					if(is_shift_pressed == 1) characterCode = 0x004C;// L
					else characterCode = 0x006C;// l
					break;
				case 0x33:
					if(is_shift_pressed == 1) characterCode = 0x003A;// ;
					else characterCode = 0x003B;// :
					break;
				case 0x34:
					if(is_shift_pressed == 1) characterCode = 0x0022;// '
					else characterCode = 0x0027;// "
					break;
				case 0x28:
					characterCode = 0x000D;// <enter>
					break;
				case 0x1D:
					if(is_shift_pressed == 1) characterCode = 0x005A;// Z
					else characterCode = 0x007A;// z
					break;
				case 0x1B:
					if(is_shift_pressed == 1) characterCode = 0x0058;// X
					else characterCode = 0x0078;// x
					break;
				case 0x06:
					if(is_shift_pressed == 1) characterCode = 0x0043;// C
					else characterCode = 0x0063;// c
					break;
				case 0x19:
					if(is_shift_pressed == 1) characterCode = 0x0056;// V
					else characterCode = 0x0076;// v
					break;
				case 0x05:
					if(is_shift_pressed == 1) characterCode = 0x0042;// B
					else characterCode = 0x0062;// b
					break;
				case 0x11:
					if(is_shift_pressed == 1) characterCode = 0x004E;// N
					else characterCode = 0x006E;// n
					break;
				case 0x10:
					if(is_shift_pressed == 1) characterCode = 0x004D;// M
					else characterCode = 0x006D;// m
					break;
				case 0x36:
					if(is_shift_pressed == 1) characterCode = 0x003C;// ,
					else characterCode = 0x002C;// <
					break;
				case 0x37:
					if(is_shift_pressed == 1) characterCode = 0x003E;// .
					else characterCode = 0x002E;// >
					break;
				case 0x38:
					if(is_shift_pressed == 1) characterCode = 0x003F;// /
					else characterCode = 0x002F;// ?
					break;
				case 0x2C:
					characterCode = 0x0020;// <space>
					break;
				case 0x50:
					characterCode = 0xFF51;// <left arrow>
					break;
				case 0x51:
					characterCode = 0xFF54;// <down arrow>
					break;
				case 0x4F:
					characterCode = 0xFF53;// <right arrow>
					break;
				case 0x52:
					characterCode = 0xFF52;// <up arrow>
					break;
				default:
					//printf("Unknown Key!!\n");
					break;
				}
				previous_character = ((u8_t *)p->payload)[4];
				if(((u8_t *)p->payload)[4] != 0)
				{
					ke.key = rfbClientSwap32IfLE(characterCode);
					if(send(sock_handle, &ke, 8, 0) == -1) printf("Couldn't send \'KeyEvent\' message!\n");
					ke.down = 0;
					if(send(sock_handle, &ke, 8, 0) == -1) printf("Couldn't send \'KeyEvent\' message!\n");
				}
			//}
		}
		/*
		fflush(stdout);
		u16_t i;
		printf("Keypress Message: ");
			for(i = 0; i < p->len; i++)
			{
				printf("0x%02X, ", ((u8_t *)p->payload)[i]);
			}
			printf("\n");*/
	}
	return ERR_OK;
}
